#include <iostream>
#include <climits>
#include <vector>
#include <math.h>
#include <LEDA/numbers/fp.h>
#include <LEDA/graphics/graphwin.h>
#include <LEDA/graphics/color.h>
#include <LEDA/core/set.h>
#include <LEDA/core/queue.h>
#include <LEDA/core/stack.h>
#include <LEDA/core/tuple.h>
#include "control.h"

#define INF -1

using namespace leda;


void clean_nodes_label(graph &g, GraphWin &gw)
{
	node v;
	//Clean up edge coloring, reset to pure matching
	forall_nodes(v , g) {
		gw.set_user_label(v , "");
	}
}

int upper_bound(int &matching_size , double &random_matching_size) {

	if ((abs(random_matching_size) - abs(matching_size)) != 0) {
		int upper_bound = 2 * floor(matching_size / (abs(random_matching_size) - abs(matching_size))) + 1;
		return upper_bound;
	}

	return 0;
}

void mark_path(GraphWin &gw , graph &g , node_array<edge> &from , node &start) {

	gw.message("Augmenting path found!");

	control_wait(1);
	node v = start;
	while (from[v] != NULL) {
		node target;
		edge e = from[v];
		target = g.opposite(e , v);

		gw.set_color(v , green);
		gw.set_color(target , green);
		gw.set_color(e , green);

		v = target;


	}

	control_wait(2);

}

void show_pure_matching(GraphWin &gw , graph &g , edge_array<bool> &match_marker) {
	node v;
	//Clean up edge coloring, reset to pure matching
	forall_nodes(v , g) {
		gw.set_color(v , grey3);
	}

	edge e;
	forall_edges(e , g) {
		if (!match_marker[e]) {
			gw.set_color(e , grey3);
		} else {
			gw.set_color(e , blue);
		}

		gw.set_style(e , solid_edge);
	}
}

int simultaneous_bfs(
		queue<node> &q,
		GraphWin &gw,
		graph &g,
		edge_array<bool>& match_marker,
		node_array<bool>& marker) {
	//Next start bfs phase to get the minimum distance from a free node to a free node
	int depth = 1;

	node_array<bool> seen_before(g,false);

	node_array<edge> from(g,NULL);

	int size = q.size();
	int new_size = 0;

	//while queue is not empty
	while (size > 0) {
		//pop until next depth level is reached
		node v = q.pop();

		size--;

		//show depth of node
		gw.set_user_label(v , string("%i" , (depth-1)));

		seen_before[v] = true;

		//Node is currently active, color it red
		gw.set_color(v, red);
		control_wait(1);

		edge e;
		//edge is used in current bfs. Mark it by changing style
		forall_inout_edges(e , v) {

			node target = opposite(e, v);

			if (!seen_before[target]) {

				//if depth mod 2 == 1 we look for marked edges
				//if depth mod 2 == 0 we look for unmarked edges
				if ((depth%2 == 1 && !match_marker[e])
				 || (depth%2 == 0 && match_marker[e])) {
					//append new node and increment size for next BFS depth
					q.append(target);
					new_size++;

					from[target] = e;

					//target is now in the bfs. Color it green
					gw.set_color(target, green);
					control_wait(1);
				}

				//a free node in V2 was found for the first time
				// => It must be the minimal distance. Return it.
				if (depth % 2 == 1 && !marker[target]) {
					//show depth of node
					gw.set_user_label(target , string("%i" , depth));
					show_pure_matching(gw,g,match_marker);
					mark_path(gw , g , from, target);
					clean_nodes_label(g, gw);
					return depth;
				}

			}
		}

		gw.set_color(v, orange);
		//gw.set_label(v , string("%i" , depth));
		control_wait(1);

		//next depth level reached. 'active' next part of queue and set size.
		if (size == 0) {
			depth++;

			size = new_size;
			new_size = 0;
		}
	}

	//no path was found. Return -1 to indicate so.
	return -1;
}




/*
 * 	Get a call_stack (alternating path) and augment it
 *
 */
void augment(
		GraphWin &gw ,
		graph &g ,
		stack<edge> &call_stack ,
		node_array<bool>&marker ,
		edge_array<bool>&match_marker,
		set<node> &freeV1,
		set<node> &freeV2,
		int &matching_size) {

	gw.message("Augmenting path found. Flipping edges.");
	control_wait(1);
	//Augmenting path found => matching size increases by 1
	matching_size++;

	edge e;
	while (!call_stack.empty()) {
		e = call_stack.pop();
		if (match_marker[e]) {
			gw.set_width(e , 1);
			gw.set_color(e , grey3);
			match_marker[e] = false;
		} else {
			//this edge has to be part of the matching.

			//color it appropriately
			gw.set_width(e , 3);
			gw.set_color(e , blue);

			//mark edge
			match_marker[e] = true;

			//mark source and target as not free
			node source = g.source(e);
			node target = g.target(e);
			marker[source] = true;
			marker[target] = true;

			//gw.set_label(source , "X");
			//gw.set_label(target , "X");

			//make blue border for nodes that are no longer free
			gw.set_border_width(source , 3);
			gw.set_border_color(source , blue);

			gw.set_border_width(target , 3);
			gw.set_border_color(target , blue);

			control_wait(1);

			//delete source and target from set of free nodes
			freeV1.del(source);
			freeV1.del(target);
		}

	}

	gw.del_message();

}

/**
 * DFS phase of Hopcroft-Karp algorithm. Gets the needed depth and tries to find augmenting paths of that length.
 *
 *
 */
void dfs(
	GraphWin &gw ,
	graph &g ,
	node &v,
	set<node> &freeV1 ,
	set<node> &freeV2 ,
	node_array<bool> &marker ,
	edge_array<bool> &match_marker ,
	int &needed_depth,
	int &current_depth,
	stack<edge> &call_stack,
	node_array<bool> &seen_before_marker,
	bool &path_found,
	int &matching_size) {

	//seen_before_marker[v] = true;

	//show depth of node
	gw.set_user_label(v , string("%i" , current_depth));

	if (current_depth == needed_depth && !marker[v]) {
		//We found an augmenting path.
		augment(gw , g , call_stack , marker , match_marker, freeV1, freeV2, matching_size);
		path_found = true;

		//gw.set_label(v, "");
		return;
	}

	//mark dfs path red
	gw.set_color(v , red);
	control_wait(0.5);

	edge e;
	forall_inout_edges(e , v) {
		node target = opposite(e , v);

		if (!seen_before_marker[target] && current_depth <= needed_depth ) {

			if (current_depth < needed_depth) {
				if ((current_depth%2 == 0 && !match_marker[e])
				||  (current_depth%2 == 1 && match_marker[e])){
					gw.set_color(e , red);
					control_wait(0.5);
					call_stack.push(e);

					//recurse => one level deeper
					current_depth++;
					//We have not seen this node before. Recurse!
					dfs(gw,g,target,freeV1,freeV2,marker,match_marker,needed_depth,current_depth,
							call_stack,seen_before_marker,path_found,matching_size);

					// clear nodes label
					//clean_nodes_label(g, gw);

					//recursion done, back one level higher
					current_depth--;

					if (path_found)
						break;

					call_stack.pop();

				}

			}
			//nothing found, color edges as inactive if not marked, or taken in marked
			if (match_marker[e]) {
				gw.set_color(e , blue);

			} else {
				gw.set_color(e , grey3);
			}

			control_wait(0.5);

		}

	}
	//node v is done, mark it as inactive
	gw.set_color(v , grey3);
	control_wait(0.5);
	//gw.set_label(v, "");

}

void hopcroft_karp(GraphWin &gw, graph &g) {

	//first get sets v1 and v2

    /* V1 and V2 are the partitions of the graph */
	//freeV1 and freeV2 contain the free nodes from V1 / V2
    set<node> V1, V2, freeV1 , freeV2;

    //Queue for first bfs phase
    queue<node> q;

    /* number of matchings */
    node v;
    // add vertices to the corresponding graph partition ( V1 V2 )
    forall_nodes(v, g)
    {
    	if(atoi(gw.get_user_label(v).c_str()) == 1){ // if label 1 add them to V1
    		V1.insert(v);
    		freeV1.insert(v);
    		q.push(v);
    		gw.set_user_label(v,"");
    	}
    	else if (atoi(gw.get_user_label(v).c_str()) == 2){ // if label 2 add them to V2
    		V2.insert(v);
    		freeV2.insert(v);
    		gw.set_user_label(v,"");
    	}
    }
    //sets V1 and V2 now contain the bipartite graph components

    //variable init

    //mark nodes that are not free anymore
    node_array<bool>marker(g,false);

    //mark edges that are currently used in the matchings
    edge_array<bool> match_marker(g,false);

    int matching_size = 0;
    double random_matching_size = g.number_of_nodes() / 2;

    gw.message(string("Starting BFS Phase. Current upper bound: %d", upper_bound(matching_size , random_matching_size)));

    //Next start bfs phase to get the minimum distance from a free node to a free node
	int depth = simultaneous_bfs(q, gw, g, match_marker, marker);

	show_pure_matching(gw,g,match_marker);

	//Do bfs / dfs until no more augmenting paths can be found
	while(depth != -1) {
		queue<node> new_q;
		gw.message(string("Found augmenting path of length %i. Starting DFS Phase. " , depth));
		control_wait(2);
		//We got the depth, now do the dfs for each free node in V1

		//marker to see if a node has been visited in another dfs
		node_array<bool> seen_before_marker(g,false);

		int curr_depth = 0;

		//start dfs from each free node
		node start;
		forall(start , freeV1) {
			stack<edge> call_stack;
			bool path_found = false;

			dfs(gw, g, start, freeV1, freeV2, marker, match_marker , depth , curr_depth,
					call_stack , seen_before_marker, path_found, matching_size);
			// clear nodes label
			clean_nodes_label(g, gw);
			if (!path_found) {
				new_q.append(start);
			}
		}

		gw.message(string("Starting BFS Phase. Current upper bound: %d", upper_bound(matching_size , random_matching_size)));

		//show pure matching
		show_pure_matching(gw,g,match_marker);

		//start / visualize BFS phase
		depth = simultaneous_bfs(new_q, gw,g, match_marker,marker);

		//show pure matching again
		show_pure_matching(gw,g,match_marker);

	}
	gw.message(string("No more augmenting paths could be found. Nr of matchings: %d",matching_size));

}


int main(int argc, char *argv[]) {

    GraphWin gw(800, 600);
    gw.display();
    create_control();
    gw.set_directed(false);
    if (argc > 1)
        gw.read(argv[1]);
    gw.edit();
    graph &g = gw.get_graph();
    if (g.number_of_nodes() == 0) {
        gw.close(); destroy_control();
        exit(1);
    }

    //color all edges and nodes grey
    edge e;
    forall_edges(e, g){
    	node v = g.target(e);
    	node s = g.source(e);

     	gw.set_width(e, 1);

        gw.set_color(e, grey3);
        gw.set_color(v , grey3);
        gw.set_color(s , grey3);

        gw.set_label_type(v, user_label);
        gw.set_label_type(s, user_label);
    }

    // start Hopcroft-Karp algorithm on g and gw
    hopcroft_karp(gw , g);


    gw.acknowledge("Ready!");
    gw.edit();
    gw.close();
    destroy_control();
    exit(0);
}
